//
// Created by jg on 2021/1/4.
//
/*   wmr_plan 机器人自主导航程序
 *   负责启动自主导航功能
 * */


#include "wmr_plan_manage/planner_manager.h"

void PlanManager::init(ros::NodeHandle &nh) {
    nh.param("planning/max_vel",                pp_.max_vel_,        -1.0);
    nh.param("planning/max_acc",                pp_.max_acc_,        -1.0);
    nh.param("manager/local_segment_length",    pp_.local_traj_len_, -1.0);
    nh.param("manager/control_points_distance", pp_.ctrl_pt_dist,    -1.0);
    nh.param("manager/thresh_replan",        replan_time_threshold_, -1.0);
    nh.param("manager/thresh_no_replan", replan_distance_threshold_, -1.0);

    nh.param("optimization/poly_order", bp_.traj_order,     10);
    nh.param("optimization/min_order",  bp_.minimize_order, 3.0);
    nh.param("planning/cube_margin",    bp_.cube_margin,    0.2);
    nh.param("planning/is_limit_vel",   bp_.is_limit_vel,  false);
    nh.param("planning/is_limit_acc",   bp_.is_limit_acc,  false);

    nh.param("safety/check_dist",       pp_.check_dist_,   0.2);

    nh.param("navigation/is_angle_PID", is_angle_PID_,   false);
    nh.param("navigation/is_active_location_path",    is_active_location_path_,  true);

    nh.param("environment/platform",  platform_,  -1);

    nh.param<std::string>("record/file_path", path_name_,
                          std::string("/home/nvidia/Data/jg/"));

    //获取当前系统的当前日期/时间
    std::time_t now = std::time(0);
    std::tm *ltm = std::localtime(&now);

    std::ostringstream ostr;
    ostr << 1 + ltm->tm_mon << "_" << ltm->tm_mday << "_" << ltm->tm_hour << "_"
         << ltm->tm_min << "_" << ltm->tm_sec << ".manager";

    std::string time_name, file_name;
    time_name = ostr.str();
    file_name = path_name_ + time_name;

    out_file_.open(file_name);
    //打开文件
    if (!out_file_) {
        std::cerr << "[manager] open output file error !" << std::endl;
        exit(-1);
    }

    cmd_pub_  = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    /* BLH 路径序列， 发送给RL_C1_driver底盘驱动， 再发送给android平板显示 */
    path_pub_ = nh.advertise<nav_msgs::Path>("/path_android",  1);

    map_edt_ = std::make_shared<MAP_EDT>();
    map_edt_->init(nh);

    path_FMM_ = std::make_shared<Path_FMM>(map_edt_);
    path_FMM_->init(nh);

    bezier_base_ = std::make_shared<Bernstein>();
    bezier_base_->init(nh);
    bezier_base_->getM(bp_.MQM, bp_.FM);
    bezier_base_->getC(bp_.C, bp_.Cv, bp_.Ca, bp_.Cj);
    global_data_.init(bezier_base_);

    topo_prm_.reset(new TopologyPRM);
    topo_prm_->init(nh, map_edt_);

    nav_VSTTM_ = std::make_shared<VSTTM>();
    nav_VSTTM_->init(nh);
    nav_VSTTM_->setEnvironment(map_edt_, bezier_base_);

    vis_rviz_ = std::make_shared<VIS_RVIZ>();
    vis_rviz_->init(nh);
    vis_rviz_->setEnv(map_edt_, bezier_base_);

    bspline_opt_.resize(10);
    for (int i = 0; i < 10; ++i) {
        bspline_opt_[i].reset(new BsplineOptimizer);
        bspline_opt_[i]->setParam(nh);
        bspline_opt_[i]->setEnvironment(map_edt_);
    }

    /* call back */
    wp_rviz_sub_ = nh.subscribe("/move_base_simple/goal", 10, &PlanManager::rcvGazeboGoal, this);
    if(platform_ == SIMULATION){
        gazebo_pos_.init(nh);

        gazebo_timer_ = nh.createTimer(ros::Duration(0.02),    &PlanManager::generateGazeboPos,     this);
        obj_sub_ = nh.subscribe("/scan",       1,      &PlanManager::rcvGazeboPcdCallback, this);
    } else if(platform_ == REAL){
        al_path_.init(is_active_location_path_);

        Eigen::Vector3d original_BLH;
        vector<double>  map_size;
        map_edt_->getOrigBLH(original_BLH);
        map_edt_->getMapSize(map_size);
        insprobe_pos_.init(map_edt_->getResolution(), original_BLH, map_size);

        wp_real_sub = nh.subscribe("/target_pose",  10,  &PlanManager::rcvRealGoal,      this);
        pos_sub_ = nh.subscribe("/insprobe/nav0", 10,  &PlanManager::rcvINSPROBECallback, this);
        obj_sub_ = nh.subscribe("/obj_pcd",       1,  &PlanManager::rcvObjPcdCallback,   this);
        grp_sub_ = nh.subscribe("/obj_ground",    1,  &PlanManager::rcvGroundPcdCallback,this);
    }

    exec_timer_ = nh.createTimer(ros::Duration(0.02), &PlanManager::execPlanCallback, this);
    safety_timer_ = nh.createTimer(ros::Duration(0.05), &PlanManager::checkCollisionCallback, this);
    vis_global_map_ = nh.createTimer(ros::Duration(0.05), &PlanManager::visGlobalMap, this);
}

void PlanManager::changeFSMExecState(PLAN_EXEC_STATE new_state, string pos_call) {
    string state_str[7] = {"INIT",
                           "WAIT_TARGET",
                           "GEN_NEW_TRAJ",
                           "REPLAN_TRAJ",
                           "EXEC_TRAJ",
                           "REPLAN_NEW",
                           "ANGLE_PID_CTRL"};
    int pre_s = int(exec_state_);
    exec_state_ = new_state;
    cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl;
}

void PlanManager::printPLANExecState() {
    string state_str[7] = {"INIT",
                           "WAIT_TARGET",
                           "GEN_NEW_TRAJ",
                           "REPLAN_TRAJ",
                           "EXEC_TRAJ",
                           "REPLAN_NEW",
                           "ANGLE_PID_CTRL"};
    cout << "state: " + state_str[int(exec_state_)] << endl;
}

void PlanManager::execPlanCallback(const ros::TimerEvent &e) {
    /* 每隔一段时间打印一次消息 */
    static bool is_first = true;
    if(is_first){
        printPLANExecState();
        is_first = false;
    }

    if(has_pos_){
        static ros::Time t_last = time_now_;
        if((time_now_ - t_last).toSec() >= 5){
            printPLANExecState();
            t_last = time_now_;
        }
    }

    switch (exec_state_) {
        case INIT:{
            if (!has_pos_) return;
            changeFSMExecState(WAIT_TARGET, "plan_node");

            break;
        }

        case WAIT_TARGET:{
            if(target_.empty()){
                stopRobot();
                return;
            } else {
                end_pos_ = target_.front();

                vis_rviz_->visTargetPt(end_pos_);
                has_target_ = true;

                changeFSMExecState(GEN_NEW_TRAJ, "plan_node");
                /*   //用于调试
                std::cout << "[plan manage] target " << end_pos_.transpose() << std::endl;
                target_.pop();
                changeFSMExecState(WAIT_TARGET, "plan_node");
                */
            }

            break;
        }

        case GEN_NEW_TRAJ:{
            if(!has_pos_ || !has_target_){
                stopRobot();
                changeFSMExecState(WAIT_TARGET, "plan_node");
                return;
            }

            bool success = planGlobalTraj();

            if(success){
                changeFSMExecState(ANGLE_PID_CTRL, "plan_node");
            } else {
                /* 全局规划失败，表明目标点选取过远，环境复杂，导致规划难度增加 */
                getSubTarget();

                ROS_WARN("[manager]: plan new trajectory fail, retrying,,, ");
                changeFSMExecState(WAIT_TARGET, "plan_node");
            }

            break;
        }

        case EXEC_TRAJ:{
            if(!has_pos_ || !has_target_){
                stopRobot();
                changeFSMExecState(WAIT_TARGET, "plan_node");
                return;
            }

            /* 是否到达全局目标点 */
            /*  是否重新规划轨迹  */
            if (isReachGlobalTarget() || isReplan()) {  //到达目标点 or 进行重规划
                return;
            }

            /* robot control algorithm : VSTTM */
            static ros::Time t_last = time_now_;
            geometry_msgs::Twist speed_command;
            /* execute velocity command each 0.1 s */
            if ((time_now_ - t_last).toSec() > 0.1 - 1e-4 ){
                nav_VSTTM_->run(speed_command.linear.x, speed_command.angular.z);
                cmd_pub_.publish(speed_command);

                t_last = time_now_;
                visBezierTraj();
            }

            break;
        }

        case REPLAN_TRAJ:{
            if(!has_pos_ || !has_target_){
                stopRobot();
                changeFSMExecState(WAIT_TARGET, "plan_node");
                return;
            }

            bool success = topoReplan();
            if(success)
                changeFSMExecState(EXEC_TRAJ, "plan_node");
            else{
                static int n_fail = 0;
                if(n_fail >= 2){
                    n_fail = 0;
                    changeFSMExecState(REPLAN_NEW, "plan_node");
                    return;
                } else n_fail++;

                ROS_WARN("[manager]: replan fail, retrying,,, %d", n_fail);
            }

            break;
        }

        case REPLAN_NEW:{
            if(!has_pos_ || !has_target_){
                stopRobot();
                changeFSMExecState(WAIT_TARGET, "plan_node");
                return;
            }

            bool success = planGlobalTraj();

            if(success)
                changeFSMExecState(ANGLE_PID_CTRL, "plan_node");
            else
                ROS_WARN("[manager]: replan new global trajectory fail, retrying,,, ");

            break;
        }

        case ANGLE_PID_CTRL:{
            if(!has_pos_ || !has_target_){
                stopRobot();
                changeFSMExecState(WAIT_TARGET, "plan_node");
                return;
            }

            if(!is_angle_PID_ || anglePIDCtrl()){
                if(is_angle_PID_)
                    ROS_INFO("[manager]: PID angle control finish !");

                changeFSMExecState(EXEC_TRAJ, "plan_node");
            }

            break;
        }
    }
}

bool PlanManager::isReachGlobalTarget(){

    double t_cur = time_now_.toSec() - global_data_.global_start_time_;
    double dis   = (start_pos_ - end_pos_).squaredNorm();
    /* 规划时间结束 或 机器人与目标点距离较近 */
    if(t_cur > global_data_.global_duration_ - 1e-1 || dis <= pow(0.1, 2)){
        file_lock_.lock();
        out_file_ << "$robot_stop," << VSTTM::double2string(time_now_.toSec()) << ","
                  << VSTTM::double2string(start_pos_[0]) << ","
                  << VSTTM::double2string(start_pos_[1]) << std::endl;
        out_file_ << "$end_pos," << VSTTM::double2string(time_now_.toSec()) << ","
                  << VSTTM::double2string(end_pos_[0]) << ","
                  << VSTTM::double2string(end_pos_[1]) << std::endl;
        file_lock_.unlock();

        target_.pop();

        stopRobot();
        ros::Duration(0.5).sleep();

        changeFSMExecState(WAIT_TARGET, "plan_node");
        local_data_.is_traj_start_ = false;

        return true;
    }

    return false;
}

bool PlanManager::isReplan() {

    double t_cur = (time_now_ - local_data_.start_time_).toSec();
    if (t_cur > replan_time_threshold_) {

        if (!global_data_.localTrajReachTarget()) {
            /* 非最终局部轨迹 */
            changeFSMExecState(REPLAN_TRAJ, "plan_node");
            return true;

        } else {
            /* 超过时间阈值，局部轨迹终点与全局轨迹终点较近 */
            Eigen::Vector3d end_pos = local_data_.position_traj_.evaluateDeBoorT(local_data_.duration_);
            if((start_pos_ - end_pos).norm() > replan_distance_threshold_){
                /* 当前位置与终点较远时 */
                changeFSMExecState(REPLAN_TRAJ, "plan_node");
                return true;
            }
        }
    }

    return false;
}

bool PlanManager::anglePIDCtrl(){
    bool last_mark = PID_finish_;
    geometry_msgs::Twist speed_command;
    nav_VSTTM_->anglePIDCtrl(PID_finish_, speed_command.linear.x, speed_command.angular.z);
    if(!PID_finish_) {
        cmd_pub_.publish(speed_command);
        return false;
    }

    if(!last_mark && PID_finish_){

        global_data_.setGlobalStartTime(time_now_);
        local_data_.start_time_    = time_now_;
        local_data_.is_traj_start_ = true;
        nav_VSTTM_->setStartTime(time_now_.toSec());
    }
    return true;
}

/* 全局运动规划： fast marching method + corridor + bezier optimization */
bool PlanManager::planGlobalTraj() {
    /* fast marching method */
    if(!FMMPathSearching()) return false;

    /* bezier trajectory optimation */
    ros::Time t_bef_opt = ros::Time::now();
    bool success = bezierTrajGenerating();
    ros::Time t_aft_opt = ros::Time::now();
    ROS_WARN("The objective of the program is %f", bp_.obj);
    ROS_WARN("The time consumation of the program is %f\n", (t_aft_opt - t_bef_opt).toSec());
    if(!success) return false;

    //bezier曲线转化为局部b-spline曲线
    double dt, duration;
    double t_cur = time_now_.toSec() - global_data_.global_start_time_;
    Eigen::MatrixXd ctrl_pts = reparamLocalTraj(t_cur, dt, duration);
    NonUniformBspline bspline(ctrl_pts, 3, dt);

    global_data_.setLocalTraj(bspline, 0.0, duration, 0.0);
    if(!is_angle_PID_){
        global_data_.setGlobalStartTime(time_now_);
        local_data_.start_time_    = time_now_;
        local_data_.is_traj_start_ = true;
    }
    local_data_.position_traj_ = bspline;

    updataLocalTraj();

    //为anglePID控制器设置目标航向角
    double t_0 = std::min(5.0, bspline.getTimeSum());
    Eigen::Vector3d pt = bspline.evaluateDeBoorT(t_0);
    double init_yaw = atan2((pt[1] - start_pos_[1]), (pt[0] - start_pos_[0]));

    nav_VSTTM_->anglePIDinit(init_yaw);
    nav_VSTTM_->setBspline(global_data_.local_traj_, time_now_.toSec());

    vis_rviz_->visBsplineTraj(bspline, 0.08, Eigen::Vector4d(1.0, 1.0, 1.0, 1), false,
                              0.15, Eigen::Vector4d(1.0, 1.0, 1.0, 1), 0, 0);
    return success;
}

bool PlanManager::FMMPathSearching() {
    vector<Eigen::Vector3d> path;
    vector<Cube> corridor;
    path_FMM_->setFMMenv(start_pos_, end_pos_);

    vector<double> time;
    path_FMM_->getfMMPath(path, time);
    path_FMM_->getCorridor(corridor, time, start_vel_);
    if(path.empty() || corridor.empty()) return false;
    vis_rviz_->visPath(path);
    vis_rviz_->visCorridor(corridor);

    global_data_.seg_num_ = corridor.size();
    global_data_.corridor.swap(corridor);
    global_data_.fmm_path.swap(path);
    global_data_.path_time.resize( global_data_.seg_num_);
    for (int i = 0; i < global_data_.seg_num_; ++i)
        global_data_.path_time[i] = global_data_.corridor[i].t;

    return true;
}

bool PlanManager::bezierTrajGenerating() {
//    if (global_data_.corridor.empty())
//        return false;

    Eigen::MatrixXd pos = Eigen::MatrixXd::Zero(2, 3);
    Eigen::MatrixXd vel = Eigen::MatrixXd::Zero(2, 3);
    Eigen::MatrixXd acc = Eigen::MatrixXd::Zero(2, 3);

    pos.row(0) = start_pos_;
    pos.row(1) = end_pos_;

    double vx = start_vel_[0];
    double vy = start_vel_[1];
    double vz = start_vel_[2];
    auto cal_v = [](double v){
        const double vlimit = 0.1;
        if(v >= -vlimit && v <= vlimit)
            return 0.0;
        return v;
    };

    vel.row(0) << cal_v(vx), cal_v(vy), cal_v(vz);
//    vel.row(0) = start_vel_;
    acc.row(0) = start_acc_;

    if(bezier_opt_.BezierPloyCoeffGeneration(global_data_.corridor, bp_.MQM,  pos, vel, acc,
                                             pp_.max_vel_, pp_.max_acc_, bp_.traj_order,
                                             bp_.minimize_order,   bp_.cube_margin,
                                             bp_.is_limit_vel,     bp_.is_limit_acc,
                                             bp_.obj,   bp_.bezier_coeff) == -1){

        ROS_WARN("Cannot find a feasible and optimal solution, somthing wrong with the optimazation solver\n");

        return false;
    } else {
        global_data_.setGlobalTraj(time_now_.toSec(), bp_);

        vis_rviz_->visBezierTraj(bp_.bezier_coeff, global_data_.path_time);

        return true;
    }

}
/* 全局运动规划： fast marching method + corridor + bezier optimization */

/* 基于topo prm + bspline 的局部轨迹重规划 */
/* 全局轨迹无碰时，直接将其转换为bspline曲线 */
bool PlanManager::topoReplan() {
    double local_traj_dt, local_traj_duration;
    double t_now = time_now_.toSec() - global_data_.global_start_time_;
    double time_inc = 0.0;

    /*截取全局轨迹 使用bspline曲线表示 作为局部轨迹*/
    Eigen::MatrixXd ctrl_pts = reparamLocalTraj(t_now, local_traj_dt, local_traj_duration);
    if(ctrl_pts.rows() < 3){ //stop
        ROS_WARN("[topo replan]: ctrl point's size is %d", (int)ctrl_pts.rows() );
        return false;
    }

    NonUniformBspline init_traj(ctrl_pts, 3, local_traj_dt);

    ros::Time t_file_begin = ros::Time::now();

    if(!collide_){
        /*无碰撞 do nothing*/
        refineTraj(init_traj, time_inc);
        local_data_.position_traj_ = init_traj;
        global_data_.setLocalTraj(init_traj, t_now, local_traj_duration + time_inc + t_now, time_inc);

    }
    else{
        plan_data_.initial_local_segment_ = init_traj;
        vector<Eigen::Vector3d> colli_start, colli_end, start_pts, end_pts;
        findCollisionRange(colli_start, colli_end, start_pts, end_pts);

        if(colli_start.empty() || colli_end.empty()){
            if(!colli_start.empty() && colli_end.empty())
                ROS_WARN("Init traj ends in obstacle, no replanning.");

            if(colli_start.empty() && colli_end.empty())
                ROS_WARN("Init traj has no collision, no replanning.");

            local_data_.position_traj_ = init_traj;
            global_data_.setLocalTraj(init_traj, t_now, local_traj_duration + t_now, 0.0);

        }else{
            plan_data_.clearTopoPaths();
            list<GraphNode::Ptr> graph;
            vector<vector<Eigen::Vector3d>> raw_paths, filtered_paths, select_paths;
            topo_prm_->findTopoPaths(colli_start.front(), colli_end.back(), start_pts, end_pts,
                                     graph, raw_paths, filtered_paths, select_paths);

            if(select_paths.empty()){
                ROS_WARN("[manager]: no topo path ");
                return false;
            }
//            for(auto& path : select_paths)
//                vis_rviz_->visPath(path);
            plan_data_.addTopoPaths(graph, raw_paths, filtered_paths, select_paths);
            plan_data_.topo_traj_pos1_.resize(select_paths.size());
            plan_data_.topo_traj_pos2_.resize(select_paths.size());

            ros::Time t_opt_begin = ros::Time::now();
            vector<thread> opt_threads;
            for (int i = 0; i < (int)select_paths.size(); ++i) {
                opt_threads.emplace_back(&PlanManager::optimizeTopoBspline, this, t_now,
                                         local_traj_duration, std::ref(select_paths[i]), i);
            }
            for (int i = 0; i < (int)select_paths.size(); ++i) opt_threads[i].join();

            ros::Time t_opt_end = ros::Time::now();
            ROS_INFO("[manage]: optimization time : %lf", (t_opt_end - t_opt_begin).toSec());

            NonUniformBspline best_traj;
            double time_inc = 0.0;
            selectBestTraj(best_traj);
            refineTraj(best_traj, time_inc);

            local_data_.position_traj_ = best_traj;
            global_data_.setLocalTraj(best_traj, t_now, local_traj_duration + time_inc + t_now, time_inc);

        }
    }
    updataLocalTraj();
    local_data_.start_time_ = time_now_;

    ros::Time t_file_end = ros::Time::now();
    file_lock_.lock();
    out_file_ << "$replan_traj_time," << VSTTM::double2string((t_file_end - t_file_begin).toSec()) << std::endl;
    file_lock_.unlock();

    nav_VSTTM_->setBspline(global_data_.local_traj_, time_now_.toSec());

    vis_rviz_->visBsplineTraj(local_data_.position_traj_, 0.08, Eigen::Vector4d(1.0, 1.0, 1.0, 1), false,
                              0.15, Eigen::Vector4d(0.0, 1.0, 1.0, 1), 0, 0);
    return true;
}

void PlanManager::findCollisionRange(vector<Eigen::Vector3d> &colli_start, vector<Eigen::Vector3d> &colli_end,
                                     vector<Eigen::Vector3d> &start_pts, vector<Eigen::Vector3d> &end_pts) {
    bool last_safe = true, safe;
    auto& initial_traj = local_data_.position_traj_;

    /* find range of collision */
    double t_s = -1.0, t_e = 0.0, dist;

    double t_now = (time_now_ - local_data_.start_time_).toSec();

    for (double tc = t_now; tc <= local_data_.duration_ - 1e-4; tc += 0.05) {

        Eigen::Vector3d ptc = initial_traj.evaluateDeBoorT(tc);
        map_edt_->getDistance(dist, ptc);
        //距离大于阈值 safe
        safe = (dist > topo_prm_->clearance_);

        if (last_safe && !safe) {
            colli_start.emplace_back(initial_traj.evaluateDeBoorT(tc - 0.05));
            if (t_s < 0.0) t_s = tc - 0.05;
        } else if (!last_safe && safe) {
            colli_end.push_back(ptc);
            t_e = tc;
        }

        last_safe = safe;
    }
    //无碰撞
    if (colli_start.empty()) return;
    //轨迹末尾被障碍物占据
    if (colli_start.size() == 1 && colli_end.empty()) return;
    /* find start and end safe segment */
    double dt = initial_traj.getInterval();
    int sn = ceil((t_s - t_now) / dt);
    dt = (t_s - t_now) / sn;

    for (double tc = t_now; tc <= t_s + 1e-4; tc += dt) {
        start_pts.emplace_back(initial_traj.evaluateDeBoorT(tc));
    }

    dt = initial_traj.getInterval();
    sn = ceil((local_data_.duration_ - t_e) / dt);
    dt = (local_data_.duration_ - t_e) / sn;

    if (dt > 1e-4) {
        for (double tc = t_e; tc <= local_data_.duration_ + 1e-4; tc += dt) {
            end_pts.emplace_back(initial_traj.evaluateDeBoorT(tc));
        }
    } else {
        end_pts.emplace_back(initial_traj.evaluateDeBoorT(local_data_.duration_ - 5e-2));
    }
}

void PlanManager::optimizeTopoBspline(double start_t, double duration,
                                      vector<Eigen::Vector3d> &guide_path, int traj_id) {
    ros::Time t1;

    double tm1, tm2, tm3;

    t1 = ros::Time::now();

    int seg_num = topo_prm_->pathLength(guide_path) / pp_.ctrl_pt_dist;
    Eigen::MatrixXd ctrl_pts;
    double dt;

    ctrl_pts = reparamLocalTraj(start_t, duration, seg_num, dt);

    auto guide = topo_prm_->pathToGuidePts(guide_path, int(ctrl_pts.rows()) - 2);
    guide.pop_back();
    guide.pop_back();
    guide.erase(guide.begin(), guide.begin() + 2);
    if ((int)guide.size() != int(ctrl_pts.rows()) - 6) ROS_WARN("what guide");

    tm1 = (ros::Time::now() - t1).toSec();
    t1 = ros::Time::now();

    // first phase, path-guided optimization
    bspline_opt_[traj_id]->setGuidePath(guide);
    auto opt_ctrl_pts1 = bspline_opt_[traj_id]->BsplineOptimizeTraj(
            ctrl_pts, dt, BsplineOptimizer::GUIDE_PHASE, 0, 1);

    plan_data_.topo_traj_pos1_[traj_id] = NonUniformBspline(opt_ctrl_pts1, 3, dt);

    tm2 = (ros::Time::now() - t1).toSec();
    t1 = ros::Time::now();

    // second phase, normal optimization
    auto opt_ctrl_pts2 = bspline_opt_[traj_id]->BsplineOptimizeTraj(
            opt_ctrl_pts1, dt, BsplineOptimizer::NORMAL_PHASE, 1, 1);

    plan_data_.topo_traj_pos2_[traj_id] = NonUniformBspline(opt_ctrl_pts2, 3, dt);

    /*************  vis rviz ***************/
//    vis_rviz_->visBsplineTraj(plan_data_.topo_traj_pos2_[traj_id], 0.08, Eigen::Vector4d(1.0, 0.0, 0.0, 1), false,
//                              0.15, Eigen::Vector4d(1.0, 1.0, 1.0, 1), 0, 0);

    tm3 = (ros::Time::now() - t1).toSec();
    ROS_INFO("optimization %d cost %lf, %lf, %lf seconds.", traj_id, tm1, tm2, tm3);
}
/* 基于topo prm + bspline 的局部轨迹重规划 */

/* 碰撞检测 */
void PlanManager::checkCollisionCallback(const ros::TimerEvent &e) {
    if (local_data_.is_traj_start_ && exec_state_ == EXEC_TRAJ ) {
        double dist;  //发生碰撞的点和机器人之间的距离
        bool safe = checkTrajCollision(dist);

        double dis_rob;
        map_edt_->getDistance(dis_rob, start_pos_);

        if (!safe) {
            if (dist > 0.5 ) {
                ROS_WARN("[manager]: %lf m to collision !", dist);
                collide_ = true;
                changeFSMExecState(REPLAN_TRAJ, "safety");
            } else {
                ROS_ERROR("[manager]: %lf m to collision, emergency stop !", dist);
                stopRobot(true);
                ros::Duration(0.5).sleep();

                collide_ = true;
                changeFSMExecState(REPLAN_NEW, "safety");
            }
            return;

        } else collide_ = false;

        /* 机器人与局部轨迹距离过远， 重新规划新的轨迹 */
        double t_now = (time_now_ - local_data_.start_time_).toSec();
        Eigen::Vector3d pt = local_data_.position_traj_.evaluateDeBoorT(t_now);
        if ((pt - start_pos_).norm() > 2) {
            changeFSMExecState(REPLAN_NEW, "safety");
        }

        file_lock_.lock();
        out_file_ << "$collision," << VSTTM::double2string(time_now_.toSec()) << ","
                  << VSTTM::double2string(dis_rob) << std::endl;
        file_lock_.unlock();

    }

}

bool PlanManager::checkTrajCollision(double &distance) {
    double t_now = (time_now_ - local_data_.start_time_).toSec();
    Eigen::Vector3d cur_pt = local_data_.position_traj_.evaluateDeBoorT(t_now);

    double radius = 0.0, dist;
    Eigen::Vector3d fut_pt;
    double fut_t = 0.02;

    while(radius < 6.0 && t_now + fut_t < local_data_.duration_) {
        fut_pt = local_data_.position_traj_.evaluateDeBoorT(t_now + fut_t);
        map_edt_->getDistance(dist, fut_pt);

        if(dist <= pp_.check_dist_){

            distance = radius;
            return false;
        }

        radius = (fut_pt - cur_pt).norm();
        fut_t += 0.02;
    }

    return true;
}
/* 碰撞检测 */

/* 画地图 */
void PlanManager::visGlobalMap(const ros::TimerEvent &e) {
    vis_rviz_->visMap(GLOBALMAP);
    if (platform_ == SIMULATION)
        vis_rviz_->visMap(LOCALMAP);
}

/* 位置信息处理 */
void PlanManager::rcvGazeboGoal(const geometry_msgs::PoseStamped::ConstPtr &msg) {
    if (msg->pose.position.z >= -1e-5) {
        // if height > 0, it's a valid goal;
        nav_msgs::Path waypoints;
        geometry_msgs::PoseStamped pt = *msg;

        waypoints.poses.clear();
        waypoints.poses.push_back(pt);

        waypoints.header.frame_id = std::string("AUTO");
        waypoints.header.stamp = msg->header.stamp;

        rcvWaypointCallback(waypoints);
    } else {
        ROS_WARN("[waypoint_generator] invalid goal in manual mode.");
    }
}

void PlanManager::generateGazeboPos(const ros::TimerEvent &e) {
    gazebo_pos_.getPosition(rob_state_);
    positionHandle(rob_state_);
}

void PlanManager::rcvINSPROBECallback(const insprobe_msgs::Navigation &msg) {
    insprobe_pos_.getLocation(msg, rob_state_);
    static ros::Time t_last = rob_state_.header.stamp;
    if((rob_state_.header.stamp - t_last).toSec() > 0.02 - 2e-3){
        positionHandle(rob_state_);
        t_last = rob_state_.header.stamp;
    }
}

void PlanManager::positionHandle(const nav_msgs::Odometry &state) {

    if (state.header.frame_id != "wmr")
        return ;

    if(!has_pos_) time_last_ = state.header.stamp;

    if(std::isnan(state.pose.pose.position.x) ||
       std::isnan(state.pose.pose.position.y) ||
       std::isnan(state.pose.pose.position.z)  ){
        has_pos_ = false;
        return;
    }

    time_now_ = state.header.stamp;
    if(has_pos_ && (time_now_ - time_last_).toSec() < 0.02 - 5e-4){
        pos_lock_.unlock();
        return;
    }

    pos_lock_.lock();
    start_pos_[0] = state.pose.pose.position.x;
    start_pos_[1] = state.pose.pose.position.y;
    start_pos_[2] = 0.01;

    /* start vel 为绝对坐标系下的速度 */
    start_vel_[0] = state.twist.twist.linear.x;
    start_vel_[1] = state.twist.twist.linear.y;
    start_vel_[2] = state.twist.twist.linear.z;

    Eigen::Quaterniond start_quan;
    start_quan.w() = state.pose.pose.orientation.w;
    start_quan.x() = state.pose.pose.orientation.x;
    start_quan.y() = state.pose.pose.orientation.y;
    start_quan.z() = state.pose.pose.orientation.z;

    Eigen::Matrix3d DCM;
    Eigen::Vector3d Eular;
    OriTrans::Quater2DCM(start_quan, DCM);
    OriTrans::DCM2Euler(DCM, Eular);

    robot_yaw_ = Eular(2);
    while(robot_yaw_ > 2*M_PI)
        robot_yaw_ -= 2*M_PI;
    while(robot_yaw_ < 0)
        robot_yaw_ += 2*M_PI;

    auto dt = (time_now_ - time_last_).toSec();
    start_acc_ = (start_vel_ - last_vel_) / dt;

    list_state_.emplace_back(ROBOTSTATE(time_now_.toSec(), start_pos_, robot_yaw_));
    if(list_state_.size() > 20)
        list_state_.pop_front();

    pos_lock_.unlock();

    has_pos_ = true;
    last_vel_  = start_vel_;
    robot_vel_ = start_vel_.norm();
    robot_avl_ = state.twist.twist.angular.z;
    time_last_ = time_now_;
    /* build local map || the time consume in build local map is arround 1ms */
    map_edt_->builtLocalMap(start_pos_);
    vis_rviz_->visRobotPos(start_pos_, robot_yaw_);
    nav_VSTTM_->setRobotState(time_now_.toSec(), start_pos_, robot_yaw_, robot_vel_, robot_avl_);
}
/* 位置信息处理 */

/* 目标点信息处理 */
void PlanManager::rcvRealGoal(const geometry_msgs::Pose2D& msg) {
    nav_msgs::Path waypoints;
    insprobe_pos_.getWayPoint(msg, waypoints);

    rcvWaypointCallback(waypoints);
}

void PlanManager::rcvWaypointCallback(const nav_msgs::Path &wp) {
    string wp_type = wp.header.frame_id;
    if(wp_type == "HANDLE"){   //手动控制模式  中止自主导航
        /* 清空目标点队列 */
        if(!target_.empty())
            queue<Eigen::Vector3d>().swap(target_);
        if(!raw_target_.empty())
            vector<Eigen::Vector3d>().swap(raw_target_);
        has_target_ = false;
        vis_rviz_->resetVisRviz();

        return;
    }
    else if(wp_type == "AUTO"){    //自动模式  自主导航
        if(wp.poses[0].pose.position.z < -1e-1) return;

        for(auto& p : wp.poses){
            Eigen::Vector3d tmp;
            tmp << p.pose.position.x, p.pose.position.y, 0.01;
            ROS_INFO("[PLAN Node]: receive the way-points: (%lf, %lf)", tmp[0], tmp[1]);

            raw_target_.push_back(tmp);
        }

        if(platform_ == REAL){
            activeLocationPath();
            has_target_ = true;
        } else {
            target_.push(raw_target_.back());
            has_target_ = true;
        }

    }

}
//active location path (by GNSS data)
void PlanManager::activeLocationPath() {

    if(raw_target_.empty())
        return;
    else if(has_pos_){
        Eigen::Vector3d start;
        if(raw_target_.size() == 1)
            start = start_pos_;
        else
            start = raw_target_[raw_target_.size()-2];

        auto path = al_path_.findPath(start, raw_target_.back());
        std::cout << "[manager]: active location path :" << std::endl;
        auto iter = path.begin();
        //赋予目标点
        for(; iter != path.end(); ++iter){
            if(iter == path.begin()) continue;
            target_.push(*iter);
            std::cout << iter->transpose() << std::endl;
        }
        //给底盘驱动发送BLH路径序列
        auto raw_path = al_path_.getRawPath();
        pubBLHPath(raw_path);
    }
}

void PlanManager::pubBLHPath(vector<Eigen::Vector3d>& _path) {
    nav_msgs::Path nav_path;
    static int seq = 0;
    nav_path.header.stamp = time_now_;
    nav_path.header.seq   = seq++;

    /* p -- BLH(deg, deg, m) */
    int _seq = 0;
    for(auto& pos : _path){
        geometry_msgs::PoseStamped p;
        insprobe_pos_.getPathPoint(pos, p);
        p.header.seq = _seq++;
        nav_path.poses.push_back(p);
    }

    path_pub_.publish(nav_path);
}
/* 目标点信息处理 */

/* 点云信息处理 */
void PlanManager::rcvGazeboPcdCallback(const sensor_msgs::LaserScan &msg) {
    sensor_msgs::PointCloud2 obj, ground;
    gazebo_pos_.getCloudPoint(msg, obj, ground);

    rcvGroundPcdCallback(ground);
    rcvObjPcdCallback(obj);
}
/*******  获取障碍物的点云  ***********/
/*******  每接收一次位置信息（使用地面点云信息）时  ***********/
/*******  对障碍物点云进行一次处理  ***********/
void PlanManager::rcvObjPcdCallback(const sensor_msgs::PointCloud2 &msg) {
    if(!has_pos_ || !map_edt_->checkPOS(start_pos_)) return;

    vector<float> vx, vy;

    pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_xyz(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(msg, *pcd_xyz);
    if (!pcd_xyz->empty()) {
        for (unsigned int i = 0; i < pcd_xyz->size(); i++) {
            if(pcd_xyz->points[i].z > 0.5) continue;

            vx.push_back(pcd_xyz->points[i].x);
            vy.push_back(pcd_xyz->points[i].y);
        }

        MyPcd::Ptr mypcd;
        mypcd = std::make_shared<MyPcd>(msg.header.stamp.toSec(), vx, vy);
        obs_pcd_.push(mypcd);

        ros::Time t1 = ros::Time::now();
        //使用点云数据刷新地图
        refreshMap();
        ros::Time t2 = ros::Time::now();
        out_file_ << "$refresh_map_time," << VSTTM::double2string(time_now_.toSec()) << ","
                  << VSTTM::double2string((t2 - t1).toSec()) << ","
                  << VSTTM::double2string(time_now_.toSec() - mypcd->time)
                  << std::endl;
    }


}
/*******  获取地面的点云  ***********/
void PlanManager::rcvGroundPcdCallback(const sensor_msgs::PointCloud2 &msg) {
    if (!use_ground_) use_ground_ = true;     //一旦接收地面点云，表明使用地面点云更新栅格地图

    vector<float> vx, vy;

    pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_xyz(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(msg, *pcd_xyz);
    if (!pcd_xyz->empty()) {
        for (unsigned int i = 0; i < pcd_xyz->size(); i++) {
            if(pcd_xyz->points[i].z > 0.5) continue;

            vx.push_back(pcd_xyz->points[i].x);
            vy.push_back(pcd_xyz->points[i].y);
        }
    }

    MyPcd::Ptr mypcd;
    mypcd = std::make_shared<MyPcd>(msg.header.stamp.toSec(), vx, vy);
    ground_pcd_.push(mypcd);

//    has_ground_ = true;     //标志接收到一次地面点云信息，每更新一次地图重置一次该标志
}

/* 利用点云信息刷新栅格地图 */
void PlanManager::refreshMap() {
    double msize = map_edt_->getLocalMapSize();
    double pcd_time;

    if(use_ground_){
        /* 首先使用地面点云清空地面区域，再使用障碍物点云刷新栅格地图状态 */

        // 比较障碍点云与地面点云的时间
        while(!obs_pcd_.empty() && !ground_pcd_.empty() &&
        obs_pcd_.front()->time < ground_pcd_.front()->time)
            obs_pcd_.pop();

        if(obs_pcd_.empty() || ground_pcd_.empty()) return;

        auto& obs = obs_pcd_.front();
        auto& grd = ground_pcd_.front();

        pos_lock_.lock();
        double _x, _y, yaw;
        if(platform_ == SIMULATION){ //仿真平台
            _x  = start_pos_[0];
            _y  = start_pos_[1];
            yaw = robot_yaw_;
        } else if(platform_ == REAL){ //实测实验
            ROBOTSTATE state_tmp;
            pcd_time = obs->time;
            // 定位数据（时间）与点云数据（时间）进行匹配
            // 最后队列 list_state_ 首部的定位数据与点云时间最契合
            if(list_state_.front().time <= pcd_time){

                for(auto iter = list_state_.begin(); iter != list_state_.end();){

                    if(iter->time < pcd_time){
                        state_tmp = *iter;
                        iter = list_state_.erase(iter);

                        if(iter->time == pcd_time)
                            break;
                        else if(iter->time > pcd_time){
                            list_state_.insert(iter, state_tmp);
                            break;
                        }
                    }
                    else ++iter;

                }
            }

            _x  = list_state_.front().pt[0];
            _y  = list_state_.front().pt[1];
            yaw = list_state_.front().yaw;

        } else{
            ROS_WARN("[wmr plan]: platform setting has wrong, state: %d !", platform_);
            return;
        }
        pos_lock_.unlock();

        /* 利用当前定位构建局部地图 */
        Eigen::Vector3d pos_tmp(_x, _y, 0.01);
        map_edt_->builtLocalMap(pos_tmp);

        // 容器 vec_point 容纳点云
        // true  -障碍物
        // false -地面
        vector<pair<Eigen::Vector3d, bool >> vec_point;
        Eigen::Vector3d point;

        for(unsigned int i = 0; i < grd->x.size(); i++){

            float& tx = grd->x[i];
            float& ty = grd->y[i];
            // 点云超出2倍局部地图范围 舍弃
            if(abs(tx) > msize || abs(ty) > msize) continue;

            double d = sqrt(pow(tx, 2) + pow(ty, 2));
            double a = atan2(ty, tx);
            if(d > 0.6 * 0.5 * msize) continue;   // 局部对角线

            point(0) = _x + d * cos(yaw + a);
            point(1) = _y + d * sin(yaw + a);
            point(2) = 0.1;
            vec_point.emplace_back(std::make_pair(point, false));
        }

        for(unsigned int i = 0; i < obs->x.size(); i++){
            float& tx = obs->x[i];
            float& ty = obs->y[i];
            // 点云超出2倍局部地图范围 舍弃
            if(abs(tx) > msize || abs(ty) > msize) continue;

            double d = sqrt(pow(tx, 2) + pow(ty, 2));
            double a = atan2(ty, tx);
            if(d > 1.5 * 0.5 * msize) continue;   // 局部对角线

            point(0) = _x + d * cos(yaw + a);
            point(1) = _y + d * sin(yaw + a);
            point(2) = 0.1;
            vec_point.emplace_back(std::make_pair(point, true));
        }

        map_edt_->refreshMap(_x, _y, vec_point);
    }

    if(!obs_pcd_.empty())
        obs_pcd_.pop();
    if(!ground_pcd_.empty())
        ground_pcd_.pop();

    vis_rviz_->visMap(LOCALMAP);
}
/* 点云信息处理 */

/* 停车 */
void PlanManager::stopRobot(bool is_keep_on_nav){
    geometry_msgs::Twist speed_command;

    speed_command.linear.x  = 0.0;
    speed_command.angular.z = 0.0;
    cmd_pub_.publish(speed_command);

    has_target_ = is_keep_on_nav;
    local_data_.is_traj_start_ = false;

//    ros::Duration(1.0).sleep();
}

/* 轨迹处理 */
/* 从全局轨迹中获取局部轨迹 */
Eigen::MatrixXd PlanManager::reparamLocalTraj(double start_t, double duration, int seg_num, double &dt) {
    vector<Eigen::Vector3d> point_set;
    vector<Eigen::Vector3d> start_end_derivative;

    global_data_.getTrajByDuration(start_t, duration, seg_num, point_set, start_end_derivative, dt);
    plan_data_.local_start_end_derivative_ = start_end_derivative;
    /* parameterization of B-spline */
    Eigen::MatrixXd ctrl_pts;
    NonUniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts);

    return ctrl_pts;
}

Eigen::MatrixXd PlanManager::reparamLocalTraj(double start_t, double& dt, double& duration) {
    /* get the sample points local traj within radius */
    vector<Eigen::Vector3d> point_set;
    vector<Eigen::Vector3d> start_end_derivative;

    global_data_.getTrajByRadius(start_t, pp_.local_traj_len_, pp_.ctrl_pt_dist, point_set,
                                 start_end_derivative, dt, duration);

    /* parameterization of B-spline */

    Eigen::MatrixXd ctrl_pts;
    NonUniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts);
    plan_data_.local_start_end_derivative_ = start_end_derivative;
    return ctrl_pts;
}

/* 如题 */
void PlanManager::selectBestTraj(NonUniformBspline& traj){
    //sort by jerk
    vector<NonUniformBspline> &trajs = plan_data_.topo_traj_pos2_;
    sort(trajs.begin(), trajs.end(),
         [&](NonUniformBspline &tj1, NonUniformBspline &tj2) {return tj1.getJerk() < tj2.getJerk();});

    traj = trajs.front();
}

void PlanManager::refineTraj(NonUniformBspline &best_traj, double &time_inc) {
    ros::Time t1 = ros::Time::now();
    time_inc = 0.0;
    double dt, t_inc;

    // int cost_function = BsplineOptimizer::NORMAL_PHASE | BsplineOptimizer::VISIBILITY;
    Eigen::MatrixXd ctrl_pts = best_traj.getControlPoint();
    int cost_function = BsplineOptimizer::NORMAL_PHASE;

    best_traj.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_);
    double ratio = best_traj.checkRatio();
    std::cout << "ratio: " << ratio << std::endl;
    reparamBspline(best_traj, ratio, ctrl_pts, dt, t_inc);
    time_inc += t_inc;

    ctrl_pts = bspline_opt_[0]->BsplineOptimizeTraj(ctrl_pts, dt, cost_function, 1, 1);
    best_traj = NonUniformBspline(ctrl_pts, 3, dt);
    ROS_WARN_STREAM("[Refine]: cost " << (ros::Time::now() - t1).toSec()
                                      << " seconds, time change is: " << time_inc);
}

/* 根据位置点速度是否超过最大速度 选择性进行优化 */
void PlanManager::reparamBspline(NonUniformBspline &bspline, double ratio,
                                        Eigen::MatrixXd &ctrl_pts, double &dt, double &time_inc) {
//    int prev_num = bspline.getControlPoint().rows();
    double time_origin = bspline.getTimeSum();
    int seg_num = (int)bspline.getControlPoint().rows() - 3;
    // double length = bspline.getLength(0.1);
    // int seg_num = ceil(length / pp_.ctrl_pt_dist);

    ratio = max(1.0, ratio);
    bspline.lengthenTime(ratio);
    double duration = bspline.getTimeSum();
    dt = duration / double(seg_num);
    time_inc = duration - time_origin;

    vector<Eigen::Vector3d> point_set;
    for (double time = 0.0; time <= duration + 1e-4; time += dt) {
        point_set.push_back(bspline.evaluateDeBoorT(time));
    }
    NonUniformBspline::parameterizeToBspline(dt, point_set, plan_data_.local_start_end_derivative_,
                                             ctrl_pts);
    // ROS_WARN("prev: %d, new: %d", prev_num, ctrl_pts.rows());
}

/* 更新 local_data_ */
void PlanManager::updataLocalTraj() {
    local_data_.velocity_traj_     = local_data_.position_traj_.getDerivative();
    local_data_.acceleration_traj_ = local_data_.velocity_traj_.getDerivative();
    local_data_.start_pos_         = local_data_.position_traj_.evaluateDeBoorT(0.0);
    local_data_.duration_          = local_data_.position_traj_.getTimeSum();
    local_data_.traj_id_ += 1;
}

/* 画机器人走过的bezier轨迹曲线 */
void PlanManager::visBezierTraj(){
    double t_now = time_now_.toSec() - global_data_.global_start_time_;
    Eigen::Vector3d pt = global_data_.getPosition(t_now);
    vis_rviz_->visReferenTraj(pt);
}

/* 获取次级目标点 */
void PlanManager::getSubTarget(){
    Eigen::Vector3d sub_target;
    bool delete_mark;

    double d0 = map_edt_->getLocalMapSize()/2;

    d0 = pow(d0, 2);
    auto& path = global_data_.fmm_path;
    /* 目标点在局部地图外 选全局路径与局部地图边界处的点 */
    /* 目标点在局部地图内 选全局路径的中间点 */
    if((end_pos_ - start_pos_).squaredNorm() >= d0- 1e-2){
        delete_mark = false;
        for(auto& pt : path){
            double dist2 = (pt - start_pos_).squaredNorm();
            if(dist2 >= d0 - 1e-2) break;

            sub_target = pt;
        }
    } else {
        delete_mark = true;
        unsigned int index = path.size()/2;
        sub_target = path[index];
    }

    /* 将次级目标点插入队列的最前面 */
    int target_size = target_.size();

    target_.push(sub_target);
    while (target_size-- > 0) {
        auto pt = target_.front();
        target_.pop();
        /* 是否删除原队列的第一个目标点 */
        if(delete_mark && target_size > 0){
            delete_mark = false;
            vis_rviz_->deleteLastTarget();
        }
        else
            target_.push(pt);
    }

}